/*
 * BCGenotype.cpp
 *
 *  Created on: 29/04/2013
 *      Author: agpardo
 */

#include "BirthClinic/include/BCGenotype.h"
#include "BirthClinic/include/BCSharedData.h"
#include "BirthClinic/include/Agents/BCAgent.h"
#include "BirthClinic/include/BirthClinic.h"
#include "Coord2d.h"
#include "lioutils/Mathlib.h"

BCGenotype::BCGenotype(BCAgentWorldModel* vm_, Coord2d mainCoords_) {
	_vm = vm_;

	int size =
			rand()
					% (BCSharedData::_genotypeMaxLength
							- BCSharedData::_genotypeMinLength)
					+ BCSharedData::_genotypeMinLength;

	int next_from = -1;

	std::map<int, Coord2d>::iterator it;

	for (int i = 1; i <= size; i++) {
		int from = i;
		int dir = rand() % 4;
		int to = i + 1;
		if (next_from != -1) {
			from = next_from;
		}

		BCGene gene(from, dir, to);

		next_from = computeCoordinates(gene, mainCoords_);

		if (next_from == -1) {
			int dir = gene.getDirection();
			Coord2d offset = computeOffset(from, dir);

			Coord2d newCenter;
			newCenter._x = mainCoords_._x + offset._x*gAgentWidth;
			newCenter._y = mainCoords_._y + offset._y*gAgentHeight;

			if(isInsideBirthClinic(newCenter)){
				_coords[to] = offset;
				_genome.push_back(gene);
			}
			else{
				size--;
				i--;
			}

		}
		else {
			i--;
		}
	}
	this->_index = _coords.begin();
}

BCGenotype::BCGenotype(BCAgentWorldModel* vm_, BCGenotype* parent1_,
		BCGenotype* parent2_, Coord2d mainCoords_) {

	_vm = vm_;

	int parent1Length = parent1_->getGenotypeLength();
	int point1 = rand() % parent1Length;

	int parent2Length = parent2_->getGenotypeLength();
	int point2 = rand() % parent2Length;

	int next_from = -1;

	std::map<int, Coord2d>::iterator it;

	int maxNumber = 0;
	for (int i = 0; i <= point1; i++) {
		BCGene parentGene = parent1_->getGene(i);

		int from = parentGene.getFrom();
		int dir = parentGene.getDirection();
		int to = parentGene.getTo();

		if(maxNumber != 0 && from > maxNumber){
			from = maxNumber;
		}

		if (next_from != -1) {
			from = next_from;
		}

		if(maxNumber != 0 && (to-maxNumber)>1){
			to = maxNumber +1;
		}

		BCGene newGene(from, dir, to);

		next_from = computeCoordinates(newGene, mainCoords_);

		if (next_from == -1) {
			int dir = newGene.getDirection();
			Coord2d offset = computeOffset(from, dir);

			Coord2d newCenter;
			newCenter._x = mainCoords_._x + offset._x*gAgentWidth;
			newCenter._y = mainCoords_._y + offset._y*gAgentHeight;

			if(isInsideBirthClinic(newCenter)){
				_coords[to] = offset;
				_genome.push_back(newGene);
				if(to > maxNumber){
					maxNumber = to;
				}
			}
		}
		else {
			i--;
		}
	}

	for (int i = point2; i < parent2_->getGenotypeLength(); i++) {
		BCGene parentGene = parent2_->getGene(i);

		int from = parentGene.getFrom();

		if(from > maxNumber){
			from = maxNumber;
		}

		int dir = parentGene.getDirection();

		if (next_from != -1) {
			from = next_from;
		}

		int to = maxNumber + 1;

		BCGene newGene(from, dir, to);

		next_from = computeCoordinates(newGene, mainCoords_);

		if (next_from == -1) {
			int dir = newGene.getDirection();
			Coord2d offset = computeOffset(from, dir);

			Coord2d newCenter;
			newCenter._x = mainCoords_._x + offset._x*gAgentWidth;
			newCenter._y = mainCoords_._y + offset._y*gAgentHeight;

			if(isInsideBirthClinic(newCenter)){
				_coords[to] = offset;
				_genome.push_back(newGene);
				if(to > maxNumber){
					maxNumber++;
				}
			}
			else{
				next_from = from;
			}
		}
		else {
			i--;
		}
	}

	this->_index = _coords.begin();
}

int BCGenotype::computeCoordinates(BCGene newGene_,
		Coord2d mainCoords_) {

	int next_from = -1;

	int from = newGene_.getFrom();
	int dir = newGene_.getDirection();

	Coord2d parent_coord;
	Coord2d genes_coord;
	std::map<int, Coord2d>::iterator it;

	if (from == 1) {
		parent_coord._x = 0;
		parent_coord._y = 0;
		_coords[from] = parent_coord;
	} else {
		it = _coords.find(from);
		if (it != _coords.end()) {
			parent_coord = it->second;
		} else {
			std::cerr << "Could not find the gene's parent.\n";
		}
	}

	genes_coord = computeOffset(from, dir);

	int collision = checkOverlapping(genes_coord);
	if (collision != -1) {
		next_from = collision;
	}

	return next_from;
}

Coord2d BCGenotype::computeOffset(int parent_, int connectionDir_){
	Coord2d genes_coord;
	Coord2d parent_coord;
	if (parent_ == 1) {
		parent_coord._x = 0;
		parent_coord._y = 0;
	} else {
		std::map<int, Coord2d>::iterator it;
		it = _coords.find(parent_);
		if (it != _coords.end()) {
			parent_coord = it->second;
		} else {
			std::cerr << "Could not find the gene's parent: " << parent_ << ".\n";
		}
	}

	switch (connectionDir_) {
		case 0:
			genes_coord._x = parent_coord._x - 1;
			genes_coord._y = parent_coord._y;
			break;
		case 1:
			genes_coord._x = parent_coord._x;
			genes_coord._y = parent_coord._y + 1;
			break;
		case 2:
			genes_coord._x = parent_coord._x + 1;
			genes_coord._y = parent_coord._y;
			break;
		case 3:
			genes_coord._x = parent_coord._x;
			genes_coord._y = parent_coord._y - 1;
			break;
		}

	return genes_coord;
}

bool BCGenotype::isCollision(Coord2d coords_) {
	// check collision with borders and environment objects.
	if ((coords_._x < 0) || (coords_._x + gAgentWidth >= gAreaWidth)
			|| (coords_._y < 0) || (coords_._y + gAgentHeight >= gAreaHeight)) {
		// * collision with border
		return true;
	} else {
		// * environment objects
		for (int i = 0; i != gAgentWidth; i++) {
			for (int j = 0; j != gAgentHeight; j++) {
				if (getPixel32(gAgentMaskImage, i, j)
						!= SDL_MapRGB(gEnvironmentImage->format, 0xFF, 0xFF,
								0xFF)) // opt: bounding box instead of pixel-to-pixel test.
								{
					// USELESS
					if ((coords_._x + i < 0) || (coords_._x + i >= gAreaWidth)
							|| (coords_._y + j < 0)
							|| (coords_._y + i >= gAreaHeight)) {
						return true;
					}

					Uint32 pixel = getPixel32(gEnvironmentImage, coords_._x + i,
							coords_._y + j);
					if (pixel
							!= SDL_MapRGB(gEnvironmentImage->format, 0xFF, 0xFF,
									0xFF)) {
						return true;
					}
				}
			}
		}
	}
	return false;
}

BCGene BCGenotype::getGene(int index_) {
	return _genome.at(index_);
}

bool BCGenotype::collisionWithOtherRobots(Coord2d center_){

	Coord2d topLeft = getCorner(0, center_, gAgentWidth, gAgentHeight);
	Coord2d topRight = getCorner(1, center_, gAgentWidth, gAgentHeight);
	Coord2d bottomLeft = getCorner(3, center_, gAgentWidth, gAgentHeight);

	int numAgents = gWorld->getNbOfAgent();

	for(int i = 0; i < numAgents; i++){

		BCAgentPtr agent = boost::dynamic_pointer_cast<BCAgent>(gWorld->getAgent(i));
		Coord2d agentPos = agent->getPosition();

		Coord2d agentTopLeftCor = getCorner(0, agentPos, gAgentWidth, gAgentHeight);
		Coord2d agentTopRightCor = getCorner(1, agentPos, gAgentWidth, gAgentHeight);
		Coord2d agentBottomRightCor = getCorner(2, agentPos, gAgentWidth, gAgentHeight);
		Coord2d agentBottomLeftCor = getCorner(3, agentPos, gAgentWidth, gAgentHeight);

		if( (agentTopLeftCor._x >= topLeft._x && agentTopLeftCor._x <= topRight._x) &&
				(agentTopLeftCor._y >= topLeft._y && agentTopLeftCor._y <= bottomLeft._y)){
			return true;
		}

		if( (agentTopRightCor._x >= topLeft._x && agentTopRightCor._x <= topRight._x) &&
				(agentTopRightCor._y >= topLeft._y && agentTopRightCor._y <= bottomLeft._y)){
			return true;
		}

		if( (agentBottomLeftCor._x >= topLeft._x && agentBottomLeftCor._x <= topRight._x) &&
				(agentBottomLeftCor._y >= topLeft._y && agentBottomLeftCor._y <= bottomLeft._y)){
			return true;
		}

		if( (agentBottomRightCor._x >= topLeft._x && agentBottomRightCor._x <= topRight._x) &&
				(agentBottomRightCor._y >= topLeft._y && agentBottomRightCor._y <= bottomLeft._y)){
			return true;
		}
	}

	return false;
}

void BCGenotype::addGene(BCGene gene_, Coord2d mainCoords_) {
	int size = _genome.size();
	size++;
}

int BCGenotype::checkOverlapping(Coord2d coord_) {
	std::map<int, Coord2d>::iterator it;

	for (it = _coords.begin(); it != _coords.end(); it++) {
		if (coord_._x == it->second._x && coord_._y == it->second._y) {
			return it->first;
		}
	}
	return -1;
}

void BCGenotype::printGenotypeInfo() {
	int numGenes = _genome.size();

	for (int i = 0; i < numGenes; i++) {
		int from = _genome.at(i).getFrom();
		int to = _genome.at(i).getTo();
		int dir = _genome.at(i).getDirection();
		std::cout << "Gene " << i << " From " << from << " To " << to
				<< " using " << dir << std::endl;
	}
}

void BCGenotype::logGenotypeInfo() {
	int numGenes = _genome.size();

	for (int i = 0; i < numGenes; i++) {
		int from = _genome.at(i).getFrom();
		int to = _genome.at(i).getTo();
		int dir = _genome.at(i).getDirection();
		BCSharedData::_birthClinicLogFile << "\tGene " << i << " From " << from
				<< " To " << to << " using " << dir << std::endl;
	}
}

int BCGenotype::getSize() {
	return this->_coords.size();
}

int BCGenotype::getGenotypeLength() {
	return this->_genome.size();
}

Coord2d BCGenotype::getCoordOfGene(int index_) {
	Coord2d result;
	int size = this->_coords.size();
	if (index_ <= size) {
		result = this->_coords[index_];
	}
	return result;
}

bool BCGenotype::isInsideBirthClinic(Coord2d center_){

	int numCornersInside = 0;

	Coord2d topLeftCorner = getCorner(0, center_, gAgentWidth, gAgentHeight);
	Coord2d topRightCorner = getCorner(1, center_, gAgentWidth, gAgentHeight);
	Coord2d bottomRightCorner = getCorner(2, center_, gAgentWidth, gAgentHeight);
	Coord2d bottomLeftCorner = getCorner(3, center_, gAgentWidth, gAgentHeight);

	Coord2d bcCenter;
	bcCenter._x = BCSharedData::_birthClinicCenterPosX;
	bcCenter._y = BCSharedData::_birthClinicCenterPosY;

	Coord2d bcTopLeftCorner = getCorner(0, bcCenter, BCSharedData::_birthClinicWidth, BCSharedData::_birthClinicHeight);
	Coord2d bcTopRightCorner = getCorner(1, bcCenter, BCSharedData::_birthClinicWidth, BCSharedData::_birthClinicHeight);
	Coord2d bcBottomLeftCorner = getCorner(3, bcCenter, BCSharedData::_birthClinicWidth, BCSharedData::_birthClinicHeight);

	if ((topLeftCorner._x >= bcTopLeftCorner._x
			&& topLeftCorner._x <= bcTopRightCorner._x)
			&& (topLeftCorner._y >= bcTopLeftCorner._y
			&& topLeftCorner._y <= bcBottomLeftCorner._y)) {
		numCornersInside++;
	}

	if ((topRightCorner._x >= bcTopLeftCorner._x
			&& topRightCorner._x <= bcTopRightCorner._x)
			&& (topRightCorner._y >= bcTopLeftCorner._y
			&& topRightCorner._y <= bcBottomLeftCorner._y)) {
		numCornersInside++;
	}

	if ((bottomRightCorner._x >= bcTopLeftCorner._x
			&& bottomRightCorner._x <= bcTopRightCorner._x)
			&& (bottomRightCorner._y >= bcTopLeftCorner._y
			&& bottomRightCorner._y <= bcBottomLeftCorner._y)) {
		numCornersInside++;
	}

	if ((bottomLeftCorner._x >= bcTopLeftCorner._x
			&& bottomLeftCorner._x <= bcTopRightCorner._x)
			&& (bottomLeftCorner._y >= bcTopLeftCorner._y
			&& bottomLeftCorner._y <= bcBottomLeftCorner._y)) {
		numCornersInside++;
	}

	if(numCornersInside == 4){
		return true;
	}

	return false;
}

Coord2d BCGenotype::getCorner(int id_, Coord2d center_, int width_, int height_){
	Coord2d corner;

	switch(id_){
	case 0: { // TOP LEFT CORNER
		corner._x = center_._x - width_ / 2;
		corner._y = center_._y - height_ / 2;
		break;
	}
	case 1: { // TOP RIGHT CORNER
		corner._x = center_._x + width_ / 2;
		corner._y = center_._y - height_ / 2;
		break;
	}
	case 2: { // BOTTOM RIGHT CORNER
		corner._x = center_._x + width_ / 2;
		corner._y = center_._y + height_ / 2;
		break;
	}
	case 3: { // BOTTOM LEFT CORNER
		corner._x = center_._x - width_ / 2;
		corner._y = center_._y + height_ / 2;
		break;
	}
	}

	return corner;
}

double BCGenotype::getMutationProbability(){
	return _mutationProb;
}

double BCGenotype::getMutationStepSize(){
	return _sigma;
}

void BCGenotype::updateMutationProb(){
	int size = _genome.size() +1;
	double tau = 1 / sqrt(size);
	double random = (double)Mathlib::getGaussianRand(0,1);
	double newSigma = _sigma * (double)exp(tau*random);
	_sigma = newSigma;

	random = Mathlib::getGaussianRand(0,1);
	double offset = (_sigma * random);
	_mutationProb += offset;

	if(_mutationProb < 0){
		int aux = trunc(_mutationProb);
		_mutationProb += (-1)*aux;
		_mutationProb *= (-1);
	}
	else if(_mutationProb>1){
		int aux = trunc(_mutationProb);
		_mutationProb -= aux;
	}
}
